#include "PhysicsSystem.h"
#include "../../nclgl/Matrix4.h"
#include "Renderer.h"

PhysicsSystem* PhysicsSystem::instance = 0;

PhysicsSystem::PhysicsSystem(void)	{

}

PhysicsSystem::~PhysicsSystem(void)	{
	for(int i = 0; i < allNodes.size(); ++i) {
		delete allNodes[i];
	}

	allNodes.clear();

	for(int i = 0; i < tempNodes.size(); ++i) {
		delete tempNodes[i];
	}

	tempNodes.clear();

	for(int i = 0; i < staticNodes.size(); ++i) {
		delete staticNodes[i];
	}

	staticNodes.clear();
}

void	PhysicsSystem::Update(float msec) {	
	/*std::ostringstream oss;
	oss << "Physics FPS: " << 1000.0f/msec;
	Renderer::GetRenderer().addOutput(oss.str());*/



	BroadPhaseCollisions();
	NarrowPhaseCollisions();

	for(int i = 0; i < allNodes.size(); ++i) {
		allNodes[i]->Update(msec);
	}

	for(int i = 0; i < allNodes.size(); ++i) {
		if(allNodes[i]->GetType() == EXPLOSION ||allNodes[i]->GetType() ==EXPLOSION_HASH){
			allNodes.erase(allNodes.begin() + i);
		}
	}

	tempNodes.clear();
}

void	PhysicsSystem::BroadPhaseCollisions() {

}

void	PhysicsSystem::NarrowPhaseCollisions() {

		CollisionData* cData = new CollisionData();
		if(allNodes.size()>1){
			for (vector<PhysicsNode*>::iterator it=allNodes.begin(); it!=allNodes.end(); ++it){
				for (vector<PhysicsNode*>::iterator it2=it; it2!=allNodes.end(); ++it2){
					if(it!=it2){

						if((*it)->GetAABB()->GetStatic() &&  (*it2)->GetAABB()->GetStatic()){
							continue;
						}

						if(!AABBCollision(*(*it)->GetAABB(), *(*it2)->GetAABB())){
							continue;
						}

						bool missileCol = false;

						if((*it)->GetType() == MISSILE_PLAYER || (*it)->GetType() == MISSILE_ENEMY){
							if((*it2)->GetType() == HEIGHTMAP_PARENT){
								//TERRAIN
								std::vector<CollisionVolume*> c = (*it2)->GetAABB()->children;
								if(c.size()>0){
									for (std::vector<CollisionVolume*>::iterator itsoft = c.begin(); itsoft != c.end(); ++itsoft){
										CollisionSphere* s1 = (CollisionSphere*)(*itsoft);
										CollisionSphere* s2 = (CollisionSphere*)(*it)->GetCollisionVolume();

										//Depending on types of two objects, call appropriate collision detection
										if(SphereSphereCollision(*s1, *s2, *cData)){
											(*it)->SetType(EXPLOSION);
											missileCol = true;
											break;
										}
									}
								}
							}
						}

						if((*it2)->GetType() == MISSILE_PLAYER || (*it2)->GetType() == MISSILE_ENEMY){
							if((*it)->GetType() == HEIGHTMAP_PARENT){
								//TERRAIN
								std::vector<CollisionVolume*> c = (*it)->GetAABB()->children;
								if(c.size()>0){
									for (std::vector<CollisionVolume*>::iterator itsoft = c.begin(); itsoft != c.end(); ++itsoft){
										CollisionSphere* s1 = (CollisionSphere*)(*itsoft);
										CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();

										//Depending on types of two objects, call appropriate collision detection
										if(SphereSphereCollision(*s1, *s2, *cData)){
											(*it2)->SetType(EXPLOSION);
											missileCol = true;
											break;
										}
									}
								}
							}
						}

						if(missileCol) continue;

						if((*it2)->GetType() == AI && (*it)->GetType() == MISSILE_PLAYER){
							CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
							CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();
						
							if(SphereSphereCollision(*s1, *s2, *cData)){
								//ENEMY MISSILE with player collision response goes here...
								(*it)->SetType(EXPLOSION);
								missileCol = true;
							}
						}

						if((*it)->GetType() == AI && (*it2)->GetType() == MISSILE_PLAYER){
							CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
							CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();
						
							if(SphereSphereCollision(*s1, *s2, *cData)){
								//...And here!
								(*it2)->SetType(EXPLOSION);
								missileCol = true;
							}
						}

						if((*it)->GetType() == MISSILE_ENEMY && (*it2)->GetType() == PLAYER){
							CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
							CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();
						
							if(SphereSphereCollision(*s1, *s2, *cData)){
								//ENEMY MISSILE with player collision response goes here...
								(*it)->SetType(EXPLOSION_HASH);
								missileCol = true;
							}
						}

						if((*it2)->GetType() == MISSILE_ENEMY && (*it)->GetType() == PLAYER){
							CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
							CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();
						
							if(SphereSphereCollision(*s1, *s2, *cData)){
								//...And here!
								(*it2)->SetType(EXPLOSION_HASH);
								missileCol = true;
							}
						}


						if(!missileCol && (*it)->GetResting() && (*it2)->GetResting()){
							continue;
						}

						//Handle Explosions
						if((*it)->GetType() == EXPLOSION||(*it)->GetType() == EXPLOSION_HASH){
							if((*it2)->GetType() == REGULAR) {
								//Regular object
								Vector3 dir = (*it2)->GetPosition() - (*it)->GetPosition();
								float dist = dir.Length();
								dir.Normalise();
								//(*it2)->AddExplodingForce(dir, dist);
							}else if((*it2)->GetType() == PLAYER || (*it2)->GetType() == AI) {
								//Regular object
								Vector3 dir = (*it2)->GetPosition() - (*it)->GetPosition();
								float dist = dir.Length();
								dir.Normalise();
								(*it2)->AddExplodingForce(Vector3(dir.x, 0.0f, dir.z), dist);
							}
						}
						//Handle Explosions
						if((*it2)->GetType() == EXPLOSION || (*it2)->GetType() == EXPLOSION_HASH){
							if((*it)->GetType() == REGULAR) {
								//Regular object
								Vector3 dir = (*it)->GetPosition() - (*it2)->GetPosition();
								float dist = dir.Length();
								dir.Normalise();
								(*it)->AddExplodingForce(dir, dist);
							}else if((*it)->GetType() == PLAYER || (*it)->GetType() == AI){
								Vector3 dir = (*it)->GetPosition() - (*it2)->GetPosition();
								float dist = dir.Length();
								dir.Normalise();
								(*it)->AddExplodingForce(Vector3(dir.x, 0.0f, dir.z), dist);
							}
						}

						//If neither have collision volumes, ignore, done later, as two AI nodes might collide, both without collision volumes
						if(!(*it)->GetCollisionVolume() && !(*it2)->GetCollisionVolume()){
							continue;
						}

						//If object doesn't have collision volume  ignore
						if(!(*it)->GetCollisionVolume() || !(*it2)->GetCollisionVolume()){
							continue;
						}

				///////////////////impulse method////////////////////

						switch((*it)->GetCollisionVolume()->GetType()){
						case COLLISION_SPHERE:
							switch((*it2)->GetCollisionVolume()->GetType()){
								case COLLISION_SPHERE:{	//Sphere-Sphere

									CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
									CollisionSphere* s2 = (CollisionSphere*)(*it2)->GetCollisionVolume();
									//CollisionData* cData = new CollisionData();
						
									//Depending on types of two objects, call appropriate collision detection
									if(SphereSphereCollision(*s1, *s2, *cData)){
										AddCollisionImpulse(*(*it), *(*it2), *cData);
									}
									break;}
								case COLLISION_AABB:{	//Sphere-AABB
									break;}
								case COLLISION_PLANE:{	//Sphere-Plane
									CollisionPlane* p = (CollisionPlane*)(*it2)->GetCollisionVolume();
									CollisionSphere* s1 = (CollisionSphere*)(*it)->GetCollisionVolume();
									//CollisionData* cData = new CollisionData();
									if(p->SphereInPlane(*(s1->GetPosition()), s1->m_radius, cData)){
										AddCollisionImpulse(*(*it), *(*it2), *cData);
									}
									break;}
							}
							break;
						case COLLISION_AABB:
							switch((*it2)->GetCollisionVolume()->GetType()){
								case COLLISION_SPHERE:	//AABB-Sphere
									break;
								case COLLISION_AABB:	//AABB-AABB
									break;
								case COLLISION_PLANE:{	//Plane-Plane
									CollisionPlane* p = (CollisionPlane*)(*it)->GetCollisionVolume();
									CollisionSphere* s1 = (CollisionSphere*)(*it2)->GetCollisionVolume();
									//CollisionData* cData = new CollisionData();
									if(p->SphereInPlane(*(s1->GetPosition()), s1->m_radius, cData)){
										AddCollisionImpulse(*(*it), *(*it2), *cData);
									}
									break;}
							}
							break;
						case COLLISION_PLANE:
							switch((*it2)->GetCollisionVolume()->GetType()){
								case COLLISION_SPHERE:	//Plane-Sphere
									break;
								case COLLISION_AABB:	//Plane-AABB
									break;
								case COLLISION_PLANE:	//Plane-Plane
									break;
							}
							break;
						}
					}
				}
			}
		}
		delete cData;
}

void	PhysicsSystem::AddNode(PhysicsNode* n) {
	allNodes.push_back(n);
}

void	PhysicsSystem::AddTemp(PhysicsNode* n) {
	tempNodes.push_back(n);
}

void	PhysicsSystem::AddStatic(PhysicsNode* n) {
	staticNodes.push_back(n);
	n->Update(0);
	//n->UpdateHash();
}

void	PhysicsSystem::RemoveNode(PhysicsNode* n) {
	for(vector<PhysicsNode*>::iterator i = allNodes.begin(); i != allNodes.end(); ++i) {
		if((*i) == n) {
			allNodes.erase(i);
			return;
		}
	}
}

void	PhysicsSystem::RemoveStatic(PhysicsNode* n) {
	for(vector<PhysicsNode*>::iterator i = staticNodes.begin(); i != staticNodes.end(); ++i) {
		if((*i) == n) {
			staticNodes.erase(i);
			return;
		}
	}
}

bool PhysicsSystem::SphereSphereCollision(const CollisionSphere &s0, const CollisionSphere &s1, CollisionData &collisionData) const
{
	//TODO: Optimise this...
	const float distSq = LengthSq(*(s0.GetPosition()) - *(s1.GetPosition()));
	//DBG_ASSERT(distSq > 0.00001f);

	const float sumRadius = (s0.m_radius + s1.m_radius);

	if (distSq < sumRadius*sumRadius)
	{
		//if (collisionData)
		//{
			collisionData.m_penetration = sumRadius - sqrtf(distSq);
			Vector3 relPos = *(s0.GetPosition()) - *(s1.GetPosition());
			relPos.Normalise();
			collisionData.m_normal = relPos;
			collisionData.m_point = *(s0.GetPosition()) - collisionData.m_normal
				* (s0.m_radius - collisionData.m_penetration*0.5f);
		//}
		return true; // Collision
	}
	return false; // No Collision
}

inline float PhysicsSystem::LengthSq(const Vector3 length){
	return length.x*length.x + length.y*length.y + length.z*length.z;
}

bool PhysicsSystem::AABBCollision(const CollisionAABB &cube0, const CollisionAABB &cube1) const{
	
	// Test along the y axis
	float dist = abs(cube0.physicsNode->GetPosition().y - cube1.physicsNode->GetPosition().y);
	float sum = (cube0.halfdims.y + cube1.halfdims.y);
	// If the dist , is less than the sum , we have an overlap
	if ( dist <= sum){
		dist = abs(cube0.physicsNode->GetPosition().x - cube1.physicsNode->GetPosition().x);
		sum = (cube0.halfdims.x + cube1.halfdims.x);
		if ( dist <= sum){
			float dist = abs(cube0.physicsNode->GetPosition().z - cube1.physicsNode->GetPosition().z);
			float sum = (cube0.halfdims.z + cube1.halfdims.z);
			if ( dist <= sum){
					// Overlap in all three axes so there is an intersection
				return TRUE;
			}
		}
	}
	return FALSE;
}

bool PhysicsSystem::PointInConvexPolygon(const Vector3 testPosition, Vector3 * convexShapePoints, int numPointsL) const {
	return false;
}

void PhysicsSystem::AddCollisionImpulse(PhysicsNode& c0, PhysicsNode& c1, CollisionData cd, bool applyRot0, bool applyRot1){
	const Vector3& hitPoint = cd.m_point;
	const Vector3& normal = cd.m_normal;
	float penetration = cd.m_penetration;
	
	// Some simple check code.
	float invMass0 = c0.GetInverseMass();
	float invMass1 = c1.GetInverseMass();

	//invMass0 = (!c0.GetResting()) ? 0.0f : invMass0;
	//invMass1 = (!c1.GetResting()) ? 0.0f : invMass1;

	const Matrix4 worldInvInertia0 = c0.GetInverseInertia();
	const Matrix4 worldInvInertia1 = c1.GetInverseInertia();

	// Both objects are non movable
	if((invMass0+invMass1 )==0.0) return;

	//Assumes evenly distributed mass, position is centre of mass
	Vector3 r0 = hitPoint - c0.GetPosition();
	Vector3 r1 = hitPoint - c1.GetPosition();

	Vector3 v0 = c0.GetLinearVelocity() + Vector3::Cross(c0.GetAngularVelocity(), r0);
	Vector3 v1 = c1.GetLinearVelocity() + Vector3::Cross(c1.GetAngularVelocity(), r1);

	// Relative Velocity
	Vector3 dv = v0 - v1;

	// If the objects are moving away from each other
	// we dont need to apply an impulse
	float relativeMovement = -Vector3::Dot(dv, normal);
	if(relativeMovement < -0.01f){
		return;
	}

	// NORMAL Impulse
	{
		// Coefficient of Restitution
		float e = 0.5f;
		float normDiv = Vector3::Dot(normal, normal) * ((invMass0 + invMass1) 
			+ Vector3::Dot(normal, Vector3::Cross(worldInvInertia0 * Vector3::Cross(r0, normal), r0) 
			+ Vector3::Cross(worldInvInertia1 * Vector3::Cross(r1, normal), r1)));
		float jn = -1*(1+e)*Vector3::Dot(dv, normal) / normDiv;

		// Hack fix to stop sinking
		// bias impulse proportional to penetration distance
		jn = jn + (penetration *0.1f);

		c0.SetLinearVelocity(c0.GetLinearVelocity()	+  normal * invMass0 * jn);
		if(applyRot0) c0.SetAngularVelocity(c0.GetAngularVelocity() + worldInvInertia0 * Vector3::Cross(r0 , normal * jn));

		c1.SetLinearVelocity(c1.GetLinearVelocity() -  normal * invMass1 * jn);
		if(applyRot1) c1.SetAngularVelocity(c1.GetAngularVelocity() - worldInvInertia1 * Vector3::Cross(r1, normal * jn));
	}

	// TANGENT Impulse Code
	{
		// Work out our tangent vector , with is perpendicular
		// to our collision normal
		Vector3 tangent = Vector3(0,0,0);
		tangent = dv - (normal * Vector3::Dot(dv, normal));
		tangent.Normalise();

		float tangDiv = invMass0 + invMass1
			+ Vector3::Dot(tangent, Vector3::Cross((c0.GetInverseInertia()
			* Vector3::Cross(r0 , tangent)), r0) + Vector3::Cross((c1.GetInverseInertia()
			* Vector3::Cross(r1, tangent)), r1));
		float jt = -1 * Vector3::Dot(dv, tangent) / tangDiv;
		// Clamp min/max tangental component

		// Apply contact impulse
		c0.SetLinearVelocity(c0.GetLinearVelocity() +  tangent * invMass0 * jt);
		if(applyRot0) c0.SetAngularVelocity(c0.GetAngularVelocity() + worldInvInertia0
			* Vector3::Cross(r0, tangent * jt));

		c1.SetLinearVelocity(c1.GetLinearVelocity() - tangent * invMass1 * jt);
		if(applyRot1) c1.SetAngularVelocity(c1.GetAngularVelocity() - worldInvInertia1
			* Vector3::Cross(r1, tangent * jt));
	} // TANGENT

}

void PhysicsSystem::AddCollisionImpulseLinear(PhysicsNode& c0, PhysicsNode& c1, CollisionData cd, bool applyRot0, bool applyRot1){
	const Vector3& hitPoint = cd.m_point;
	const Vector3& normal = cd.m_normal;
	float penetration = cd.m_penetration;
	
	// Some simple check code.
	float invMass0 = c0.GetInverseMass();
	float invMass1 = c1.GetInverseMass();

	//invMass0 = (!c0.GetResting()) ? 0.0f : invMass0;
	//invMass1 = (!c1.GetResting()) ? 0.0f : invMass1;

	const Matrix4 worldInvInertia0 = c0.GetInverseInertia();
	const Matrix4 worldInvInertia1 = c1.GetInverseInertia();

	// Both objects are non movable
	if((invMass0+invMass1 )==0.0) return;

	//Assumes evenly distributed mass, position is centre of mass
	Vector3 r0 = hitPoint - c0.GetPosition();
	Vector3 r1 = hitPoint - c1.GetPosition();

	Vector3 v0 = c0.GetLinearVelocity() + Vector3::Cross(c0.GetAngularVelocity(), r0);
	Vector3 v1 = c1.GetLinearVelocity() + Vector3::Cross(c1.GetAngularVelocity(), r1);

	// Relative Velocity
	Vector3 dv = v0 - v1;

	// If the objects are moving away from each other
	// we dont need to apply an impulse
	float relativeMovement = -Vector3::Dot(dv, normal);
	if(relativeMovement < -0.01f){
		return;
	}

	// NORMAL Impulse
	{
		// Coefficient of Restitution
		float e = 0.5f;
		float normDiv = Vector3::Dot(normal, normal) * ((invMass0 + invMass1) 
			+ Vector3::Dot(normal, Vector3::Cross(worldInvInertia0 * Vector3::Cross(r0, normal), r0) 
			+ Vector3::Cross(worldInvInertia1 * Vector3::Cross(r1, normal), r1)));
		float jn = -1*(1+e)*Vector3::Dot(dv, normal) / normDiv;

		// Hack fix to stop sinking
		// bias impulse proportional to penetration distance
		jn = jn + (penetration *0.1f);

		Vector3 temp1Lin = c0.GetLinearVelocity()	+  normal * invMass0 * jn;
		c0.SetLinearVelocity(Vector3(temp1Lin.x, 0, temp1Lin.z));
		if(applyRot0){
			Vector3 temp1Rot = c0.GetAngularVelocity() + worldInvInertia0 * Vector3::Cross(r0 , normal * jn);
			c0.SetAngularVelocity(Vector3(temp1Rot.x, 0, temp1Rot.z));
		}

		Vector3 temp2Lin = c1.GetLinearVelocity() -  normal * invMass1 * jn;
		c1.SetLinearVelocity(Vector3(temp2Lin.x, 0, temp2Lin.z));
		if(applyRot1){
			Vector3 temp2Rot = c1.GetAngularVelocity() - worldInvInertia1 * Vector3::Cross(r1, normal * jn);
			c1.SetAngularVelocity(Vector3(temp2Rot.x, 0, temp2Rot.z));
		}
	}

	// TANGENT Impulse Code
	{
		// Work out our tangent vector , with is perpendicular
		// to our collision normal
		Vector3 tangent = Vector3(0,0,0);
		tangent = dv - (normal * Vector3::Dot(dv, normal));
		tangent.Normalise();

		float tangDiv = invMass0 + invMass1
			+ Vector3::Dot(tangent, Vector3::Cross((c0.GetInverseInertia()
			* Vector3::Cross(r0 , tangent)), r0) + Vector3::Cross((c1.GetInverseInertia()
			* Vector3::Cross(r1, tangent)), r1));
		float jt = -1 * Vector3::Dot(dv, tangent) / tangDiv;
		// Clamp min/max tangental component

		// Apply contact impulse
		Vector3 temp1Lin = c0.GetLinearVelocity() +  tangent * invMass0 * jt;
		c0.SetLinearVelocity(Vector3(temp1Lin.x, 0, temp1Lin.z));
		if(applyRot0){
			Vector3 temp1Rot = c0.GetAngularVelocity() + worldInvInertia0 * Vector3::Cross(r0, tangent * jt);
			c0.SetAngularVelocity(Vector3(temp1Rot.x, 0, temp1Rot.z));
		}

		Vector3 temp2Lin = c1.GetLinearVelocity() - tangent * invMass1 * jt;
		c1.SetLinearVelocity(Vector3(temp2Lin.x, 0, temp2Lin.z));
		if(applyRot1){
			Vector3 temp2Rot = c1.GetAngularVelocity() - worldInvInertia1 * Vector3::Cross(r1, tangent * jt);
			c1.SetAngularVelocity(Vector3(temp2Rot.x, 0, temp2Rot.z));
		}
	} // TANGENT

}